This pull request addresses issue #6 by extending support for controlling the Unitree Z1 robotic arm when it is mounted on a quadruped; e.g., using the aliengoZ1_description model in unitree_ros.

Issue:

Failed attempt to control Z1 arm joints within aliengoZ1_description in Gazebo

Currently, the z1_controller repository provides a ROS control interface for the Unitree Z1 robotic arm in the /z1_gazebo namespace:

Relevant ROS nodes and topics during operation of z1_description in Gazebo by z1_controller

However, there is a need to extend support for controlling the Z1 arm when it is mounted on a quadruped model with a different namespace. For example, using the aliengoZ1_description model where the namespace is /aliengoZ1_gazebo:

Relevant ROS nodes and topics during attempted operation of Z1 arm within aliengoZ1_description in Gazebo by z1_controller; note dead and leaf topics due to namespace mismatch

The issue stems from a hard-coding of the /z1_gazebo namespace within the Z1 control interface for ROS Gazebo simulation, IOROS:

Proposed Changes:

Implement a mechanism to automatically detect the correct namespace based on the model being used when z1_controller begins execution, and use it to configure the namespace of ROS topics for Z1 joint control.

Insights:

• Unitree's quadruped models (e.g., aliengoZ1_description) initialize a ROS parameter called /robot_name in their launch files, each specifying a model's unique namespace.
• The /robot_name parameter can be retrieved and used to configure the IOROS interface with the correct namespace for Z1 joints.
  • Similar to Unitree's implementations in servo.cpp and move_publisher.cpp.
• Note that z1.launch does not initialize a /robot_name parameter; the original /z1_gazebo namespace should be used in this case.

Implementation details:

• Added a private member variable _rname of type std::string to the IOROS class.
  • During IOROS initialization, _rname stores /robot_name retrieved from the ROS parameter server, defaulting to "z1" if no such parameter is found.
• _rname configures the namespace for Z1 joints whenever IOROS::_initSend() or IOROS::_initRecv() are invoked.

Results:

The modification maintains z1_controller's functionality for control of z1_description in Gazebo, while enabling the capability of z1_controller to control the Z1 arm when it exists in a different robot namespace:

Relevant ROS nodes and topics during operation of z1_description in Gazebo by updated z1_controller	Relevant ROS nodes and topics during successful operation of Z1 arm within aliengoZ1_description in Gazebo by updated z1_controller

Update enables simultaneous control of quadruped and Z1 arm joints in Gazebo; unitree_servo and z1_controller's demo trajectory visualized here